// --------------------------------------------------------------------------------------------------------------------
// <copyright file="URControlClientTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Hardware.UniversalRobots
{
    using System;
    using System.Threading.Tasks;
    using Microsoft.Robotics.Hardware.UniversalRobots;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Tests for the <c>URControlClient</c> class.
    /// </summary>
    [TestClass]
    public class URControlClientTest
    {
        /// <summary>
        /// Packet to be returned by mock server for test
        /// </summary>
        private byte[] feedbackPacket =
        {
            0,
            0,
            4,
            230,
            16,
            0,
            0,
            0,
            29,
            0,
            0,
            0,
            0,
            0,
            0,
            1,
            98,
            177,
            1,
            1,
            1,
            1,
            0,
            0,
            0,
            5,
            63,
            240,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            251,
            1,
            191,
            230,
            228,
            168,
            252,
            112,
            126,
            0,
            191,
            230,
            228,
            208,
            135,
            60,
            197,
            40,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            189,
            192,
            212,
            227,
            66,
            63,
            153,
            154,
            65,
            222,
            102,
            103,
            66,
            54,
            0,
            0,
            240,
            191,
            252,
            91,
            232,
            252,
            131,
            69,
            8,
            191,
            252,
            91,
            252,
            193,
            233,
            104,
            152,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            189,
            146,
            235,
            87,
            66,
            64,
            0,
            0,
            65,
            224,
            0,
            0,
            66,
            59,
            153,
            154,
            240,
            191,
            247,
            109,
            1,
            2,
            58,
            128,
            5,
            191,
            247,
            109,
            1,
            3,
            241,
            186,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            188,
            146,
            235,
            87,
            66,
            64,
            0,
            0,
            65,
            224,
            0,
            0,
            66,
            63,
            153,
            154,
            240,
            192,
            11,
            172,
            55,
            103,
            216,
            234,
            111,
            192,
            11,
            172,
            59,
            226,
            180,
            95,
            7,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            61,
            46,
            231,
            103,
            66,
            63,
            153,
            154,
            66,
            0,
            102,
            103,
            66,
            84,
            204,
            205,
            240,
            191,
            218,
            24,
            20,
            237,
            123,
            29,
            32,
            191,
            218,
            24,
            88,
            226,
            247,
            179,
            48,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            61,
            53,
            38,
            135,
            66,
            63,
            153,
            154,
            65,
            255,
            51,
            51,
            66,
            83,
            51,
            51,
            240,
            191,
            230,
            126,
            179,
            73,
            99,
            211,
            160,
            191,
            230,
            127,
            111,
            26,
            137,
            29,
            48,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            62,
            17,
            59,
            148,
            66,
            63,
            153,
            154,
            66,
            4,
            204,
            205,
            66,
            89,
            51,
            51,
            240,
            0,
            0,
            0,
            53,
            4,
            63,
            205,
            170,
            162,
            188,
            131,
            142,
            192,
            191,
            220,
            140,
            241,
            49,
            228,
            0,
            96,
            63,
            215,
            150,
            135,
            19,
            75,
            39,
            127,
            63,
            251,
            137,
            61,
            36,
            151,
            32,
            73,
            63,
            229,
            143,
            129,
            67,
            47,
            240,
            186,
            191,
            240,
            116,
            72,
            70,
            193,
            108,
            72,
            0,
            0,
            0,
            225,
            5,
            21,
            56,
            226,
            255,
            223,
            0,
            234,
            191,
            255,
            82,
            43,
            31,
            56,
            226,
            246,
            255,
            23,
            142,
            2,
            223,
            63,
            159,
            91,
            255,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            191,
            219,
            51,
            51,
            51,
            51,
            51,
            51,
            191,
            217,
            26,
            159,
            190,
            118,
            200,
            180,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            63,
            182,
            211,
            31,
            205,
            36,
            225,
            97,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            63,
            187,
            241,
            65,
            32,
            91,
            192,
            26,
            63,
            184,
            58,
            251,
            126,
            144,
            255,
            151,
            63,
            181,
            17,
            156,
            224,
            117,
            246,
            253,
            63,
            249,
            33,
            251,
            84,
            82,
            69,
            80,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            63,
            249,
            33,
            251,
            84,
            82,
            69,
            80,
            191,
            249,
            33,
            251,
            84,
            82,
            69,
            80,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            53,
            9,
            63,
            243,
            16,
            107,
            214,
            189,
            242,
            12,
            64,
            56,
            40,
            13,
            106,
            199,
            172,
            70,
            64,
            47,
            197,
            55,
            251,
            229,
            159,
            147,
            191,
            235,
            146,
            42,
            211,
            184,
            53,
            30,
            191,
            210,
            60,
            123,
            144,
            10,
            196,
            238,
            191,
            245,
            70,
            91,
            14,
            79,
            204,
            150,
            0,
            0,
            0,
            64,
            3,
            0,
            0,
            0,
            0,
            0,
            0,
            63,
            68,
            1,
            64,
            20,
            1,
            64,
            20,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            65,
            225,
            153,
            154,
            66,
            65,
            153,
            154,
            62,
            215,
            149,
            0,
            0,
            0,
            0,
            0,
            5,
            2,
            0,
            0,
            0,
            0,
            37,
            2,
            0,
            0,
            63,
            155,
            23,
            252,
            197,
            136,
            112,
            92,
            63,
            153,
            127,
            252,
            246,
            38,
            15,
            102,
            66,
            54,
            0,
            0,
            0,
            59,
            68,
            155,
            166,
            66,
            68,
            0,
            0,
            239,
            0,
            0,
            1,
            213,
            6,
            192,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            64,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            192,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            64,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            192,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            64,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            192,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            64,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            192,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            64,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            192,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            64,
            25,
            33,
            251,
            84,
            68,
            45,
            24,
            64,
            9,
            51,
            51,
            51,
            51,
            51,
            51,
            64,
            46,
            0,
            0,
            0,
            0,
            0,
            0,
            64,
            9,
            51,
            51,
            51,
            51,
            51,
            51,
            64,
            46,
            0,
            0,
            0,
            0,
            0,
            0,
            64,
            9,
            51,
            51,
            51,
            51,
            51,
            51,
            64,
            46,
            0,
            0,
            0,
            0,
            0,
            0,
            64,
            9,
            153,
            153,
            153,
            153,
            153,
            154,
            64,
            57,
            0,
            0,
            0,
            0,
            0,
            0,
            64,
            9,
            153,
            153,
            153,
            153,
            153,
            154,
            64,
            57,
            0,
            0,
            0,
            0,
            0,
            0,
            64,
            9,
            153,
            153,
            153,
            153,
            153,
            154,
            64,
            57,
            0,
            0,
            0,
            0,
            0,
            0,
            63,
            240,
            193,
            82,
            56,
            45,
            115,
            101,
            63,
            246,
            87,
            24,
            74,
            231,
            68,
            135,
            63,
            208,
            0,
            0,
            0,
            0,
            0,
            0,
            63,
            243,
            51,
            51,
            51,
            51,
            51,
            51,
            63,
            208,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            191,
            219,
            51,
            51,
            51,
            51,
            51,
            51,
            191,
            217,
            26,
            159,
            190,
            118,
            200,
            180,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            63,
            182,
            211,
            31,
            205,
            36,
            225,
            97,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            63,
            187,
            241,
            65,
            32,
            91,
            192,
            26,
            63,
            184,
            58,
            251,
            126,
            144,
            255,
            151,
            63,
            181,
            17,
            156,
            224,
            117,
            246,
            253,
            63,
            249,
            33,
            251,
            84,
            82,
            69,
            80,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            63,
            249,
            33,
            251,
            84,
            82,
            69,
            80,
            191,
            249,
            33,
            251,
            84,
            82,
            69,
            80,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            5,
            0,
            0,
            0,
            2,
            0,
            0,
            0,
            1,
            0,
            0,
            0,
            1,
            0,
            0,
            0,
            2,
            0,
            0,
            0,
            2,
            0,
            0,
            0,
            2,
            0,
            0,
            0,
            2,
            0,
            0,
            0,
            2,
            0,
            0,
            0,
            2,
            0,
            0,
            0,
            61,
            7,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            63,
            75,
            29,
            68,
            161,
            45,
            191,
            175,
            0,
            0,
            0,
            7,
            8,
            0,
            1
        };

        /// <summary>
        /// Packet to be returned by mock real time server for test
        /// </summary>
        private byte[] realTimePacket =
        {
            0,
            0,
            3,
            44,
            64,
            220,
            212,
            9,
            55,
            75,
            198,
            168,
            191,
            233,
            247,
            150,
            122,
            74,
            249,
            43,
            191,
            248,
            248,
            121,
            150,
            213,
            125,
            66,
            63,
            63,
            197,
            139,
            132,
            245,
            176,
            43,
            191,
            249,
            250,
            137,
            151,
            49,
            34,
            26,
            63,
            247,
            255,
            93,
            170,
            141,
            177,
            71,
            191,
            209,
            49,
            21,
            52,
            6,
            33,
            58,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            187,
            252,
            222,
            94,
            48,
            95,
            157,
            235,
            191,
            136,
            78,
            40,
            77,
            218,
            40,
            186,
            63,
            147,
            30,
            135,
            72,
            113,
            153,
            90,
            63,
            166,
            178,
            184,
            21,
            3,
            171,
            57,
            191,
            29,
            56,
            71,
            75,
            91,
            175,
            1,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            188,
            54,
            213,
            36,
            223,
            30,
            128,
            0,
            191,
            194,
            237,
            173,
            173,
            24,
            169,
            186,
            63,
            206,
            81,
            139,
            193,
            45,
            244,
            91,
            63,
            218,
            42,
            196,
            93,
            172,
            135,
            174,
            191,
            81,
            11,
            223,
            29,
            184,
            13,
            239,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            191,
            233,
            247,
            103,
            168,
            29,
            227,
            160,
            191,
            248,
            248,
            139,
            92,
            190,
            116,
            139,
            63,
            62,
            145,
            84,
            145,
            9,
            64,
            0,
            191,
            249,
            250,
            124,
            147,
            109,
            30,
            20,
            63,
            247,
            255,
            81,
            28,
            126,
            155,
            137,
            191,
            209,
            47,
            166,
            83,
            108,
            187,
            64,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            63,
            163,
            156,
            67,
            245,
            211,
            206,
            215,
            191,
            98,
            93,
            106,
            239,
            196,
            136,
            207,
            191,
            189,
            215,
            205,
            197,
            159,
            94,
            81,
            191,
            148,
            169,
            24,
            77,
            189,
            25,
            233,
            63,
            170,
            140,
            68,
            70,
            102,
            25,
            164,
            191,
            176,
            201,
            163,
            164,
            252,
            204,
            115,
            63,
            206,
            165,
            176,
            179,
            38,
            206,
            139,
            64,
            35,
            110,
            118,
            196,
            206,
            178,
            77,
            192,
            2,
            99,
            123,
            74,
            151,
            17,
            72,
            63,
            243,
            254,
            178,
            30,
            95,
            129,
            239,
            17,
            0,
            0,
            0,
            100,
            209,
            127,
            182,
            112,
            102,
            103,
            183,
            128,
            208,
            127,
            182,
            1,
            0,
            0,
            0,
            199,
            197,
            102,
            183,
            128,
            208,
            127,
            182,
            212,
            83,
            94,
            183,
            1,
            0,
            0,
            0,
            172,
            238,
            87,
            183,
            2,
            0,
            0,
            0,
            199,
            197,
            102,
            183,
            1,
            0,
            0,
            0,
            128,
            197,
            102,
            183,
            128,
            197,
            102,
            183,
            164,
            208,
            127,
            182,
            39,
            0,
            88,
            183,
            128,
            197,
            102,
            183,
            199,
            197,
            102,
            183,
            1,
            0,
            0,
            0,
            100,
            209,
            127,
            182,
            244,
            191,
            102,
            183,
            10,
            0,
            0,
            0,
            128,
            197,
            102,
            183,
            200,
            208,
            127,
            182,
            213,
            249,
            87,
            183,
            128,
            197,
            102,
            183,
            199,
            197,
            102,
            183,
            1,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            191,
            238,
            214,
            210,
            103,
            19,
            205,
            39,
            63,
            241,
            79,
            230,
            109,
            36,
            206,
            165,
            191,
            165,
            176,
            50,
            11,
            56,
            127,
            93,
            191,
            194,
            179,
            105,
            54,
            220,
            125,
            58,
            63,
            229,
            123,
            177,
            193,
            11,
            69,
            86,
            63,
            226,
            194,
            182,
            73,
            19,
            209,
            125,
            191,
            158,
            183,
            13,
            190,
            14,
            27,
            93,
            191,
            193,
            85,
            122,
            217,
            227,
            12,
            211,
            63,
            239,
            235,
            51,
            249,
            224,
            128,
            41,
            191,
            238,
            194,
            39,
            238,
            103,
            195,
            96,
            63,
            253,
            139,
            202,
            37,
            184,
            41,
            79,
            191,
            255,
            62,
            9,
            129,
            76,
            49,
            80,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            64,
            63,
            25,
            153,
            160,
            0,
            0,
            0,
            64,
            63,
            25,
            153,
            160,
            0,
            0,
            0,
            64,
            62,
            25,
            153,
            160,
            0,
            0,
            0,
            64,
            66,
            51,
            51,
            64,
            0,
            0,
            0,
            64,
            66,
            166,
            102,
            96,
            0,
            0,
            0,
            64,
            67,
            166,
            102,
            96,
            0,
            0,
            0,
            63,
            128,
            151,
            200,
            8,
            65,
            237,
            225,
            64,
            32,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            64,
            111,
            160,
            0,
            0,
            0,
            0,
            0,
            64,
            111,
            160,
            0,
            0,
            0,
            0,
            0,
            64,
            111,
            160,
            0,
            0,
            0,
            0,
            0,
            64,
            111,
            160,
            0,
            0,
            0,
            0,
            0,
            64,
            111,
            160,
            0,
            0,
            0,
            0,
            0,
            64,
            111,
            160,
            0,
            0,
            0,
            0,
            0
        };

        /// <summary>
        /// Expected joint angles based on above packet.
        /// </summary>
        private double[] expectedJointAngles = 
        {
            -0.71541046,
            -1.77238186,
            -1.46415671,
            -3.45906805,
            -0.407708913,
            -0.703193156
        };

        /// <summary>
        /// Expected TCP pose based on above packet.
        /// </summary>
        private Pose expectedTCPPose = new Pose(
            new Vector3(
                0.231775,
                -0.4461002,
                0.368554),
            Quaternion.FromAxisAngle(
            new AxisAngle(
                new Vector3(
                    0.813659794447966,
                    0.318608562160653,
                    -0.486257465769746),
                    2.115134619829197)));

        /// <summary>
        /// String for move home command
        /// </summary>
        private string moveHomeCommandString = "movej([0, -1.57, 0, -1.57, 0, 0])\n";

        /// <summary>
        /// Test parsing and publishing of feedback from <c>URControlClient</c>
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Slow")]
        [Priority(0)]
        public void TestURControlClientFeedback()
        {
            MockURControlClientNetworkConnection mockURServerNetworkConnection = new MockURControlClientNetworkConnection(
                this.feedbackPacket,
                -1);

            MockURControlClientNetworkConnection mockRealtimeNetworkConnection = new MockURControlClientNetworkConnection(
                this.realTimePacket,
                -1);

            URControlClient controlClient = new URControlClient(mockURServerNetworkConnection, mockRealtimeNetworkConnection);

            controlClient.Connect();

            Assert.IsTrue(controlClient.IsConnected);

            // Necessary to ensure that properties of <c>controlClient</c> have been updated.
            Task.Delay(500).Wait();

            JointChainData jointChainData = controlClient.JointChainData;

            for (int i = 0; i < 6; ++i)
            {
                Assert.AreEqual(this.expectedJointAngles[i], jointChainData.JointData[i].ActualPositionInRadians, 0.001);
                Assert.AreEqual(this.expectedJointAngles[i], jointChainData.JointData[i].TargetPositionInRadians, 0.001);
            }

            ManipulatorStatus status = controlClient.ManipulatorStatus;

            // all but last joint should have zero velocity
            for (int i = 0; i < 5; ++i)
            {
                Assert.AreEqual(0, status.JointVelocities[i]);
            }

            Assert.AreNotEqual(0, status.JointVelocities[5]);

            CartesianData cartesianData = controlClient.TcpCartesianData;

            Assert.AreEqual(this.expectedTCPPose.Position.X, cartesianData.CartesianPose.Position.X, 0.0001);
            Assert.AreEqual(this.expectedTCPPose.Position.Y, cartesianData.CartesianPose.Position.Y, 0.0001);
            Assert.AreEqual(this.expectedTCPPose.Position.Z, cartesianData.CartesianPose.Position.Z, 0.0001);
            Assert.AreEqual(this.expectedTCPPose.Orientation.W, cartesianData.CartesianPose.Orientation.W, 0.0001);
            Assert.AreEqual(this.expectedTCPPose.Orientation.X, cartesianData.CartesianPose.Orientation.X, 0.0001);
            Assert.AreEqual(this.expectedTCPPose.Orientation.Y, cartesianData.CartesianPose.Orientation.Y, 0.0001);
            Assert.AreEqual(this.expectedTCPPose.Orientation.Z, cartesianData.CartesianPose.Orientation.Z, 0.0001);

            ControllerData controllerData = controlClient.ControllerData;

            Assert.AreEqual(1, controllerData.SpeedFraction);

            TcpForceData forceData = controlClient.TcpForceData;

            Assert.AreEqual(0.0, forceData.X);

            ConfigurationData configurationData = controlClient.ConfigurationData;

            Assert.AreEqual(5, configurationData.MasterboardVersion);

            AdditionalInfo additionalInfo = controlClient.AdditionalInfo;

            Assert.AreEqual(false, additionalInfo.IsTeachButtonPressed);

            MasterboardData masterboardData = controlClient.MasterboardData;

            Assert.AreEqual(false, masterboardData.IsEuromap67Installed);

            controlClient.Dispose();
        }

        /// <summary>
        /// Test sending a command
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void TestURControlClientCommand()
        {
            MockURControlClientNetworkConnection mockURServerNetworkConnection = new MockURControlClientNetworkConnection(
                this.feedbackPacket,
                -1);

            MockURControlClientNetworkConnection mockRealtimeNetworkConnection = new MockURControlClientNetworkConnection(
                this.realTimePacket,
                -1);

            URControlClient controlClient = new URControlClient(mockURServerNetworkConnection, mockRealtimeNetworkConnection);

            controlClient.Connect();

            Assert.IsTrue(controlClient.IsConnected);

            // command for moving to home position
            controlClient.SendCommand(this.moveHomeCommandString);
            byte[] expectedResult = System.Text.Encoding.ASCII.GetBytes(this.moveHomeCommandString);

            for (int i = 0; i < expectedResult.Length; ++i)
            {
                Assert.AreEqual(expectedResult[i], mockURServerNetworkConnection.LastCommandReceived[i]);
            }

            controlClient.Dispose();
        }
    }
}
